/**
 *
 * SimpleFOCMini motor control example
 * 
 * For Arduino UNO or the other boards with the UNO headers
 * the most convenient way to use the board is to stack it to the pins:
 * - 12 - ENABLE
 * - 11 - IN1
 * - 10 - IN2
 * -  9 - IN3
 *
 */

#if 1
#include <Arduino.h>
#include <SimpleFOC.h>
#include "bldc_init.h"

extern BLDCMotor motor0;
extern BLDCMotor motor1;

extern Commander command;
extern float g_target_velocity;

void setup() {
  config_motor0();
  config_motor1();
  config_bldc_cmd();
}

void loop() {
  // main FOC algorithm function
  // the faster you run this function the better
  // Arduino UNO loop  ~1kHz
  // Bluepill loop ~10kHz
  motor0.loopFOC();
  motor1.loopFOC();
  // Motion control function
  // velocity, position or voltage (defined in motor.controller)
  // this function can be run at much lower frequency than loopFOC() function
  // You can also use motor.move() and set the motor.target in the code
  motor0.move(g_target_velocity);
  motor1.move(g_target_velocity);
  // function intended to be used with serial plotter to monitor motor variables
  // significantly slowing the execution down!!!!
  // motor.monitor();

  // user communication
  command.run();
}

#endif 
